#include "motor.h"

extern TIM_HandleTypeDef htim2;                        //定时器2初始化句柄


/* 左侧电机占空比设置 值为 [-1000, +1000] */
static void motor_pwm_set_l(int p)                                       //左轮电机
{
  if (p >= 0)                                                     //控制电机顺时针旋转
  {
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_0, GPIO_PIN_SET);     //AIN2
  }
  else                                                                 //控制电机逆时针旋转
  {
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_2, GPIO_PIN_SET);                //AIN1
    HAL_GPIO_WritePin(GPIOF, GPIO_PIN_0, GPIO_PIN_RESET);   
    p = -p;
  }
  __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, p);
}


/* 右侧电机pwm */
static void motor_pwm_set_r(int p)                                        //右轮电机
{
  if (p >= 0)                                                       //控制电机顺时针旋转
  {
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_SET);                 //BIN1
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_RESET);               //BIN2
  }
  else                                                                  //控制电机逆时针旋转
  {
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOC, GPIO_PIN_1, GPIO_PIN_SET);
    p = -p;
  }
  __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, p);
}


/* 前进速度，差速值r>0右转 */
void motor_move(int s, int r)
{
  motor_pwm_set_l(s + r);
  motor_pwm_set_r(s - r);
}

